#include <ros/ros.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include <std_msgs/Char.h>
#include "cyphy_kbd/KeyCode.h"
#define KEYCODE_SPACE 0x20 
#define KEYCODE_ESC 0x1B
#define KEYCODE_s 0x73

class CyphyKbd
{
public:
  CyphyKbd();
  void keyLoop();
private:
  ros::NodeHandle nh_;
  ros::Publisher key_pub_;
};

CyphyKbd::CyphyKbd()
{
  key_pub_ = nh_.advertise<cyphy_kbd::KeyCode>("keycode", 10);
}

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "cyphy_kbd");
  CyphyKbd cyphy_kbd;

  signal(SIGINT,quit);

  cyphy_kbd.keyLoop();
  
  return(0);
}


void CyphyKbd::keyLoop()
{
  char c;


  // get the console in raw mode                                                              
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");
  puts("---------------------------");
  //puts("Use arrow keys to move the turtle.");


  for(;;)
  {
    // get the next event from the keyboard  
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }
    
    printf("value: 0x%02X\n", c);

	#if 0
    switch(c)
    {
      case KEYCODE_SPACE:
        printf("Space bar\n");
        break;
      case KEYCODE_ESC:
        printf("ESC\n");
        break;
	  case KEYCODE_s:
        printf("s\n");
        break;
    }
	#endif
	

   cyphy_kbd::KeyCode code; 
   code.data = c;
   key_pub_.publish(code);    
  }
  return;
}





